/*
 * driveRover.cpp
 *
 *  Created on: Nov 11, 2012
 *      Author: bouchier
 */
/*! \file jRoverMotors.h
 * \ brief The file containing code for control of jRover's motors.
 */

#include <iostream>
#include <cmath>
#include "CQEI2C.h"
#include "ControlledMotor.h"
#include "JRoverMotors.h"

// Robot physical parameters in inches
#define WIDTH 18
#define LENGTH_FRONT 10.5
#define LENGTH_BACK 14.75
#define WHEEL_CIRCUMFERENCE 2 * PI * 2
#define PI 3.14159265359
#define TWOPI 2 * PI

extern CQEI2C i2c;
int jRoverNumMotors = 12;						// default to jRover - all motors present

/*! \def MOTOR_NUMBERS
 * Wiring assumptions:
 * Motor ports:
 * 15: left front drive
 * 3: left front steer
 * 16: right front drive
 * 1: right front steer
 * 2: left middle drive
 * 7: left middle steer
 * 12: right middle drive
 * 6: right middle steer
 * 14: left back drive
 * 4: left back steer
 * 13: right back drive
 * 5: right back steer
 */

// Motor numbers, in order of encoder wiring
#define LM_STEER 7
#define LM_DRIVE 2
#define LB_DRIVE 14
#define LB_STEER 4
#define LF_DRIVE 15
#define LF_STEER 3
#define RM_STEER 6
#define RB_DRIVE 13
#define RB_STEER 5
#define RM_DRIVE 12
#define RF_STEER 1
#define RF_DRIVE 16

// PID parameters for motors 1 - 16 (on servo ports & H-bridge for both servo & motor use)
#define DRIVE_HMOTOR_KP 15.0
#define DRIVE_HMOTOR_KI 2.0
#define DRIVE_HMOTOR_KD 1.0
#define DRIVE_SMOTOR_KP 10.0
#define DRIVE_SMOTOR_KI 1.0
#define DRIVE_SMOTOR_KD 0.0
#define SERVO_SMOTOR_KP 2.0
#define SERVO_SMOTOR_KI 1.0
#define SERVO_SMOTOR_KD 1.0
//#define SERVO_SMOTOR_KP 3.5
//#define SERVO_SMOTOR_KI 2.0
//#define SERVO_SMOTOR_KD 2.0
#define SERVO_HMOTOR_KP 10.0
#define SERVO_HMOTOR_KI 1.0
#define SERVO_HMOTOR_KD 0.0

// instantiate the controlled motors.
ControlledMotor lfDrive = ControlledMotor(LF_DRIVE, i2c, true, WHEEL_CIRCUMFERENCE);
ControlledMotor lfSteer = ControlledMotor(LF_STEER, i2c, true, true, 140);
ControlledMotor rfDrive = ControlledMotor(RF_DRIVE, i2c, false, WHEEL_CIRCUMFERENCE);
ControlledMotor rfSteer = ControlledMotor(RF_STEER, i2c, true, false, 140);
ControlledMotor lmDrive = ControlledMotor(LM_DRIVE, i2c, true, WHEEL_CIRCUMFERENCE);
ControlledMotor lmSteer = ControlledMotor(LM_STEER, i2c, true, true, 136);
ControlledMotor rmDrive = ControlledMotor(RM_DRIVE, i2c, false, WHEEL_CIRCUMFERENCE);
ControlledMotor rmSteer = ControlledMotor(RM_STEER, i2c, true, false, 139);
ControlledMotor lbDrive = ControlledMotor(LB_DRIVE, i2c, true, WHEEL_CIRCUMFERENCE);
ControlledMotor lbSteer = ControlledMotor(LB_STEER, i2c, true, false, 140);
ControlledMotor rbDrive = ControlledMotor(RB_DRIVE, i2c, false, WHEEL_CIRCUMFERENCE);
ControlledMotor rbSteer = ControlledMotor(RB_STEER, i2c, true, true, 140);

JRoverMotors::JRoverMotors()
{
}

void fatal(char *op, int motNum)
{
	printf("ERROR %s motor %d\n", op, motNum);
	exit(0);
}

//! initialize all motors, in order of the I2C chain
/*!
 * WARNING: do not change the order of the I2C chain without changing the order init()
 * is called in this function.
 */
void JRoverMotors::initMotors()
{
	printf("Initializing all motors\n");
	if (!lmSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", LM_STEER);
	if (!lmDrive.init(DRIVE_SMOTOR_KP, DRIVE_SMOTOR_KI, DRIVE_SMOTOR_KD)) fatal((char *)"initializing", LM_DRIVE);
	if (!lbDrive.init(DRIVE_HMOTOR_KP, DRIVE_HMOTOR_KI, DRIVE_HMOTOR_KD)) {
		jRoverNumMotors = 2;
		printf("Only found 2 motors, must be running on test platform\n");
		return;
	}
	if (!lbSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", LB_STEER);
	if (!lfDrive.init(DRIVE_HMOTOR_KP, DRIVE_HMOTOR_KI, DRIVE_HMOTOR_KD)) fatal((char *)"initializing", LF_DRIVE);
	if (!lfSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", LF_STEER);
	if (!rmSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", RM_STEER);
	if (!rbDrive.init(DRIVE_HMOTOR_KP, DRIVE_HMOTOR_KI, DRIVE_HMOTOR_KD)) fatal((char *)"initializing", RB_DRIVE);
	if (!rbSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", RB_STEER);
	if (!rmDrive.init(DRIVE_SMOTOR_KP, DRIVE_SMOTOR_KI, DRIVE_SMOTOR_KD)) fatal((char *)"initializing", RM_DRIVE);
	if (!rfSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD)) fatal((char *)"initializing", RF_STEER);
	if (!rfDrive.init(DRIVE_HMOTOR_KP, DRIVE_HMOTOR_KI, DRIVE_HMOTOR_KD)) fatal((char *)"initializing", RF_DRIVE);

	if (1) {	// parallel servo zeroing
		driveAllSteerToStop();
		driveAllSteerToCenter();
		zeroAllEncoders();
	}
}

//! Drive all motors to mech stop
void JRoverMotors::driveAllSteerToStop()
{
	lfSteer.driveToMechStop();
	lmSteer.driveToMechStop();
	lbSteer.driveToMechStop();
	rfSteer.driveToMechStop();
	rmSteer.driveToMechStop();
	rbSteer.driveToMechStop();
	sleep(1);
}

//! Drive all motors to center
void JRoverMotors::driveAllSteerToCenter()
{
	lfSteer.driveToStraight();
	lmSteer.driveToStraight();
	lbSteer.driveToStraight();
	rfSteer.driveToStraight();
	rmSteer.driveToStraight();
	rbSteer.driveToStraight();
	for (int i=0; i<80; i++) {
		updateAllMotors();
		usleep(30000);
	}
	// in theory, all motors are now straight
}

//! Zero & clear all encoders
void JRoverMotors::zeroAllEncoders()
{
	lfSteer.zeroEncoder();
	lmSteer.zeroEncoder();
	lbSteer.zeroEncoder();
	rfSteer.zeroEncoder();
	rmSteer.zeroEncoder();
	rbSteer.zeroEncoder();

}

//! Set all drive motors running at the given speed
void setAllDriveSpeed(float rqSpeed)
{
	lfDrive.setSpeed(rqSpeed);
	rfDrive.setSpeed(rqSpeed);
	lmDrive.setSpeed(rqSpeed);
	rmDrive.setSpeed(rqSpeed);
	lbDrive.setSpeed(rqSpeed);
	rbDrive.setSpeed(rqSpeed);
}

//! Set all steering motors to the given angle
void setAllSteerAngle(int rqDegrees)
{
	lfSteer.setDegrees(rqDegrees);
	rfSteer.setDegrees(rqDegrees);
	lmSteer.setDegrees(rqDegrees);
	rmSteer.setDegrees(rqDegrees);
	lbSteer.setDegrees(rqDegrees);
	rbSteer.setDegrees(rqDegrees);
}

//! Run PID to update motor power on all motors & keep them at the requested speed/position
void JRoverMotors::updateAllMotors()
{
	if (!lmSteer.updateMotor()) fatal((char *)"updating", LM_STEER);
	if (!lmDrive.updateMotor()) fatal((char *)"updating", LM_DRIVE);
	if (jRoverNumMotors == 2) {return;}				// if test platform
	if (!lbDrive.updateMotor()) fatal((char *)"updating", LB_DRIVE);
	if (!lbSteer.updateMotor()) fatal((char *)"updating", LB_STEER);
	if (!lfDrive.updateMotor()) fatal((char *)"updating", LF_DRIVE);
	if (!lfSteer.updateMotor()) fatal((char *)"updating", LF_STEER);
	if (!rmSteer.updateMotor()) fatal((char *)"updating", RM_STEER);
	if (!rbDrive.updateMotor()) fatal((char *)"updating", RB_DRIVE);
	if (!rbSteer.updateMotor()) fatal((char *)"updating", RB_STEER);
	if (!rmDrive.updateMotor()) fatal((char *)"updating", RM_DRIVE);
	if (!rfSteer.updateMotor()) fatal((char *)"updating", RF_STEER);
	if (!rfDrive.updateMotor()) fatal((char *)"updating", RF_DRIVE);
}

//! Get requested speed, actual speed, distance, power of a motor
/*
 * \param motNum The motor ID to get data from
 * \param m A pointer to the ControlledMotor object for this motor
 */
void JRoverMotors::getDriveMotorTelemetry(int motNum, ControlledMotor *m)
{
	driveMotorTelemetry[motNum][0] = m->getRqSpeed();
	driveMotorTelemetry[motNum][1] = m->getSpeed();
	driveMotorTelemetry[motNum][2] = m->getDistance();
	driveMotorTelemetry[motNum][3] = (int)(m->getMotorPower());
}

//!get telemetry for all  drive motors
void JRoverMotors::getDriveMotorsTelemetry()
{
	if (jRoverNumMotors == 2) {
		getDriveMotorTelemetry(0, &lmDrive);
		return;
	}
	// get the data from each motor
	getDriveMotorTelemetry(0, &lfDrive);
	getDriveMotorTelemetry(1, &lmDrive);
	getDriveMotorTelemetry(2, &lbDrive);
	getDriveMotorTelemetry(3, &rfDrive);
	getDriveMotorTelemetry(4, &rmDrive);
	getDriveMotorTelemetry(5, &rbDrive);
}

//! Set test motor telemetry in the drive motor telemetry array
void JRoverMotors::setTestDriveMotorTelemetry(float f)
{
	int i, j;

	for (i=0; i<6; i++) {
		for (j=0; j<4; j++) {
			driveMotorTelemetry[i][j] = (float)i + f + ((float)j)/10.0;
		}
	}
}

//! Get requested angle, actual angle, power of a motor being used as a servo
/*
 * \param motNum The motor ID to get data from
 * \param m A pointer to the ControlledMotor object for this motor
 */
void JRoverMotors::getSteerMotorTelemetry(int motNum, ControlledMotor *m)
{
	steerMotorTelemetry[motNum][0] = m->getRqDegrees();
	steerMotorTelemetry[motNum][1] = m->getDegrees();
	steerMotorTelemetry[motNum][2] = m->getMotorPower();
}

//!get telemetry for all steering motors
void JRoverMotors::getSteerMotorsTelemetry()
{
	if (jRoverNumMotors == 2) {
		getSteerMotorTelemetry(0, &lmDrive);
		return;
	}
	// get the data from each motor
	getSteerMotorTelemetry(0, &lfSteer);
	getSteerMotorTelemetry(1, &lmSteer);
	getSteerMotorTelemetry(2, &lbSteer);
	getSteerMotorTelemetry(3, &rfSteer);
	getSteerMotorTelemetry(4, &rmSteer);
	getSteerMotorTelemetry(5, &rbSteer);
}

//! Set test motor telemetry in the steering motor telemetry array
void JRoverMotors::setTestSteerMotorTelemetry(float f)
{
	int i, j;

	for (i=0; i<6; i++) {
		for (j=0; j<3; j++) {
			steerMotorTelemetry[i][j] = i*10 + (int)f + j;
		}
	}
}

//! Request jRover to drive with given linear & angular velocity
/*
 * driveRover takes a requested robot linear and angular velocity and asks each motor to drive
 * to the appropriate position/speed to move the robot as requested. It uses the following
 * steering and wheel speed equations:
 *
 * ---
 * R = linear/angular
 *
 * R: robot turn radius in inches
 * linear: robot linear speed at center of middle wheels in in/s;
 * angular: robot angular velocity in rad/s
 * ---
 *
 * vRM = linear + 0.5 * (angular * WIDTH)
 * vLM = linear - 0.5 * (angular * WIDTH)
 *
 * vRM, vLM: speed of right-middle, left-middle motors in in/s
 * WIDTH: distance between adjacent wheels
 * ---
 *
 * thetaLF = atan(LENGTH_FRONT/(R - (0.5 * WIDTH))) * 180 / PI
 * thetaRF = atan(LENGTH_FRONT/(R + (0.5 * WIDTH))) * 180 / PI
 * thetaLB = atan(LENGTH_BACK/(R - (0.5 * WIDTH))) * 180 / PI
 * thetaRB = atan(LENGTH_BACK/(R + (0.5 * WIDTH))) * 180 / PI
 *
 * thetaLF, thetaRF, thetaLB, thetaRB: angle of left-front, right-front,
 *   left-back, right-back steering wheels in degrees
 * LENGTH_FRONT: distance between middle and front axles
 * LENGTH_BACK: distance between middle and back axles
 * Note inaccuracy; if LENGTH_FRONT/BACK changes as wheels steer the calculation will be incorrect
 * ---
 *
 * vLF = angular * (LENGTH_FRONT/sin(thetaLF))
 * vRF = angular * (LENGTH_FRONT/sin(thetaRF))
 * vLB = angular * (LENGTH_BACK/sin(thetaLB))
 * vRB = angular * (LENGTH_BACK/sin(thetaRB))
 *
 * vLF, vRF, vLB, vRB: speed of left-fron, right-front, left-back, right-back
 *   wheels in in/s
 *
 * =====
 * Test stimuli & expected results
 *
 * $ rostopic pub driveRover -1 jrover/Velocity -- 10 0.1
 * Requested linear speed: 10.00, requested angular speed: 0.10, turn radius: 100.0
 * LF: (9.80, 5.9)         RF: (10.30, 5.6)
 * LM: 9.75                RF: 10.25
 * LB: (9.95, -11.6)       RB: (10.44, -11.0)
 *
 * $ rostopic pub driveRover -1 jrover/Velocity -- 10 -0.1
 * Requested linear speed: 10.00, requested angular speed: -0.10, turn radius: -100.0
 * LF: (10.30, -5.6)       RF: (9.80, -5.9)
 * LM: 10.25               RF: 9.75
 * LB: (10.44, 11.0)       RB: (9.95, 11.6)
 *
 * $ rostopic pub driveRover -1 jrover/Velocity -- -10 0.1
 * Requested linear speed: -10.00, requested angular speed: 0.10, turn radius: -100.0
 * LF: (-10.30, -5.6)      RF: (-9.80, -5.9)
 * LM: -10.25              RF: -9.75
 * LB: (-10.44, 11.0)      RB: (-9.95, 11.6)
 *
 * $ rostopic pub driveRover -1 jrover/Velocity -- -10 -0.1
 * Requested linear speed: -10.00, requested angular speed: -0.10, turn radius: 100.0
 * LF: (-9.80, 5.9)                RF: (-10.30, 5.6)
 * LM: -9.75               RF: -10.25
 * LB: (-9.95, -11.6)              RB: (-10.44, -11.0)
 *
 * \param linear The requested linear speed in in/s
 * \param angular The requested angular velocity in rad/s
 * \param quiet True (default) to quietly request motor speed/angle settings, false to print requested setting for each motor
 * \return 0 for success, -1 if the request resulted in trying to drive any motor beyond it's limit
 */
void JRoverMotors::driveRover(float linear, float angular, bool quiet)
{
	float R;		// robot turn radius in inches
	float vLM, vRM; //speed of right-middle, left-middle motors in in/s
	int thetaLF, thetaRF, thetaLB, thetaRB;	// angle of left-front, right-front, left-back, right-back steering wheels in degrees
	float thetaLFr, thetaRFr, thetaLBr, thetaRBr;	// angle of left-front, right-front, left-back, right-back steering wheels in radians
	float vLF, vRF, vLB, vRB; // speed of left-front, right-front, left-back, right-back wheels in in/s

	// if we're not really trying to move, leave steering angle alone, stop motors
	if ((linear < 1.0) && (linear > -1.0)) {
		lfDrive.setSpeed(0.0);
		rfDrive.setSpeed(0.0);
		lmDrive.setSpeed(0.0);
		rmDrive.setSpeed(0.0);
		lbDrive.setSpeed(0.0);
		rbDrive.setSpeed(0.0);
		return;
	} else if ((angular < 0.01) && (angular > -0.01))	{		// call it straight, avoid the divide by 0 if angular is 0
		if (!quiet)
			printf("Requested linear speed: %0.1f, requested angular speed < 0.01\n", linear);
		vRM = vLM = linear;
		vLF = vRF = linear * 1.02;
		vLB = vRB = linear * 0.98;
		thetaLF = thetaRF = thetaLB = thetaRB = 0;
	} else {
		// calculate robot turn radius
		R = linear/angular;
		if (!quiet)
			printf("Requested linear speed: %0.1f, requested angular speed: %0.2f, turn radius: %0.1f\n", linear, angular, R);

		// calculate middle drive motor speeds
		vRM = linear + 0.5 * (angular * WIDTH);
		vLM = linear - 0.5 * (angular * WIDTH);

		// calculate steered wheel angles
		thetaLFr = atan(LENGTH_FRONT/(R - (0.5 * WIDTH)));
		thetaRFr = atan(LENGTH_FRONT/(R + (0.5 * WIDTH)));
		thetaLBr = 0 - (atan(LENGTH_BACK/(R - (0.5 * WIDTH))));
		thetaRBr = 0 - (atan(LENGTH_BACK/(R + (0.5 * WIDTH))));
		thetaLF = (int)(thetaLFr * 180 / PI);
		thetaRF = (int)(thetaRFr * 180 / PI);
		thetaLB = (int)(thetaLBr * 180 / PI);
		thetaRB = (int)(thetaRBr * 180 / PI);

		// calculate steered wheel speeds
		vLF = angular * (LENGTH_FRONT/sin(thetaLFr));
		vRF = angular * (LENGTH_FRONT/sin(thetaRFr));
		vLB = angular * (LENGTH_BACK/sin(0 - thetaLBr));
		vRB = angular * (LENGTH_BACK/sin(0 - thetaRBr));
	}

	if (!quiet) {
		printf("LF: (%0.2f, %d)\t\tRF: (%0.2f, %d)\n", vLF, thetaLF, vRF, thetaRF);
		printf("LM: %0.2f\t\tRF: %0.2f\n", vLM, vRM);
		printf("LB: (%0.2f, %d)\t\tRB: (%0.2f, %d)\n", vLB, thetaLB, vRB, thetaRB);
	}

	// command the motors to run
	lfDrive.setSpeed(vLF);
	lfSteer.setDegrees(thetaLF);
	rfDrive.setSpeed(vRF);
	rfSteer.setDegrees(thetaRF);
	lmDrive.setSpeed(vLM);
	lmSteer.setDegrees(0);
	rmDrive.setSpeed(vRM);
	rmSteer.setDegrees(0);
	lbDrive.setSpeed(vLB);
	lbSteer.setDegrees(thetaLB);
	rbDrive.setSpeed(vRB);
	rbSteer.setDegrees(thetaRB);
}
